#include "ego_planner_adapter/trajectory_tracker.hpp"
#include <rclcpp/rclcpp.hpp>
#include <memory>

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    
    // Create node options
    rclcpp::NodeOptions options;
    
    // Create the trajectory tracker node
    auto node = std::make_shared<ego_planner_adapter::TrajectoryTracker>(options);
    
    // Create executor
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(node->get_node_base_interface());
    
    // Configure the node
    if (node->configure().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
        RCLCPP_INFO(node->get_logger(), "TrajectoryTracker configured successfully");
        
        // Activate the node
        if (node->activate().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
            RCLCPP_INFO(node->get_logger(), "TrajectoryTracker activated successfully");
            
            try {
                // Spin the executor
                executor.spin();
            } catch (const std::exception& e) {
                RCLCPP_ERROR(node->get_logger(), "Exception during execution: %s", e.what());
            }
            
            // Deactivate the node
            node->deactivate();
            RCLCPP_INFO(node->get_logger(), "TrajectoryTracker deactivated");
        } else {
            RCLCPP_ERROR(node->get_logger(), "Failed to activate TrajectoryTracker");
        }
        
        // Cleanup the node
        node->cleanup();
        RCLCPP_INFO(node->get_logger(), "TrajectoryTracker cleaned up");
    } else {
        RCLCPP_ERROR(node->get_logger(), "Failed to configure TrajectoryTracker");
    }
    
    rclcpp::shutdown();
    return 0;
}